#include "cybertron/sensor/noise/IMUNoiseMaker.hpp"
#include "cybertron/sensor/noise/LinearTimeNoise.hpp"
#include "cybertron/sensor/noise/PeriodicTimeNoise.hpp"
#include "cybertron/sensor/noise/PinkNoise.hpp"
#include "cybertron/sensor/noise/BiasNoise.hpp"
#include "cybertron/sensor/noise/BrownNoise.hpp"
#include "cybertron/core/Log.hpp"

CYBERTRON_BEGIN

IMUNoiseMaker::IMUNoiseMaker(GPSNoiseMakerPtr pGPSNoise, float positionRandomWalk/* = 10.0 meter per square hour*/, float angularRandomWalk/* = 0.00524 rad per square hour*/)
	: m_pGPSNoise(pGPSNoise), 
	m_imuVelocityNoise(0.0, pGPSNoise->velocityError()),
	m_imuAccelerationNoise(0.0, pGPSNoise->sensorConfig().AccelerationError),
	m_imuAngularVelocityNoise(0.0, pGPSNoise->sensorConfig().AngularVelocityError),
	mGpsSignalSelector(0, 1.0f)
{
	m_pGPSNoise->update(position(), rotation());
	m_pGPSNoise->addUpdateHandler([&]{
		syncWithGPSNoise();
	});
	if (pGPSNoise->horizontalError() > 0 || pGPSNoise->verticalError() > 0) {
		m_imuPositionNoise.addNoise(std::make_shared<BiasNoise>(m_pGPSNoise->positionNoise())); // use gps position
		m_imuPositionNoise.addNoise(std::make_shared<LinearTimeNoise>(3600 / positionRandomWalk, 3600.0f)); // bias to one direction.
		m_imuPositionNoise.addNoise(std::make_shared<PinkNoise>(100.0f, pGPSNoise->horizontalError(), pGPSNoise->verticalError(), 0.0f));
		m_imuPositionNoise.addNoise(std::make_shared<BrownNoise>(positionRandomWalk));
	}

	if (m_pGPSNoise->attitudeError() > 0) {
		m_imuAttitudeNoise.addNoise(std::make_shared<BiasNoise>(m_pGPSNoise->attitudeNoise())); // use gps attitude
		m_imuAttitudeNoise.addNoise(std::make_shared<BrownNoise>(angularRandomWalk));
	}

	m_lastAttitude = vec3(0.0f);
	m_lastAttitudeUpdateTime = 0.0f;
}

IMUNoiseMaker::IMUNoiseMaker(const SensorGpsImuConfig& sensorConfig)
	: IMUNoiseMaker(std::make_shared<GPSNoiseMaker>(sensorConfig))
{
}

IMUNoiseMaker::~IMUNoiseMaker()
{
}

void IMUNoiseMaker::update(const vec3& pos, const vec3& rotation, uint64_t frame/* = 0*/)
{
	// update gps first.
	if (m_pGPSNoise->sensorConfig().HzGps > 0) {
		int gpsInterval = (int)(m_pGPSNoise->sensorConfig().HzImu / m_pGPSNoise->sensorConfig().HzGps);
		bool isGpsOutage = m_pGPSNoise->sensorConfig().IsGpsOutage ||
			mGpsSignalSelector(NoiseBase::randomEngine()) < 1.0f - m_pGPSNoise->sensorConfig().GpsSignal;
		if (frame % gpsInterval == 0 && !isGpsOutage) {
			m_pGPSNoise->update(pos, rotation, frame);
		}
	}

	m_imuPositionNoise.update();
	m_imuVelocityNoise.update();
	m_imuAttitudeNoise.update();
	m_imuAngularVelocityNoise.update();
	m_imuAccelerationNoise.update();
	float headingChange = std::abs(m_lastAttitude.z - rotation.z);
	if (headingChange > NOISE_PI) headingChange = (float)(NOISE_PI * 2 - headingChange);
	bool bHeadingChanged = false;
	if (length(m_lastAttitude) > 0 && headingChange > 0.04) {
		bHeadingChanged = true;
		if (auto pLinearTimeNoise = std::dynamic_pointer_cast<LinearTimeNoise>(m_imuPositionNoise.getNoise(ELinearTimeNoise))) {
			pLinearTimeNoise->drift(m_lastAttitude.z < rotation.z ? headingChange : -headingChange);
		}
	}
	float timeInterval = frame / 60.f - m_lastAttitudeUpdateTime;
	if (bHeadingChanged || timeInterval > 0.099999f) {
		m_lastAttitude = rotation;
		m_lastAttitudeUpdateTime = frame / 60.f;
	}
	
	NoiseMaker::update(pos, rotation);
}

void IMUNoiseMaker::syncWithGPSNoise()
{
	// may need consider kalman filter here.
	m_imuPositionNoise.resetNoise();
	if (auto pBiasNoise = std::dynamic_pointer_cast<BiasNoise>(m_imuPositionNoise.getNoise(EBiasNoise))) {
		pBiasNoise->setBias(m_pGPSNoise->positionNoise());
	}
	m_imuAttitudeNoise.resetNoise();
	if (auto pBiasNoise = std::dynamic_pointer_cast<BiasNoise>(m_imuAttitudeNoise.getNoise(EBiasNoise))) {
		pBiasNoise->setBias(m_pGPSNoise->attitudeNoise());
	}
}

CYBERTRON_END